#include "path_plan.h"
#include "robot_common.h"
#include <opencv2\imgcodecs.hpp>
PathPlan::PathPlan()
{
}

PathPlan::~PathPlan()
{
}
bool
PathPlan::getDestLoc(LocationDH input_ori, std::vector<LocationDH> input_dest, std::vector<LocationDH>& output)
{
	for (int i = 0; i < input_dest.size(); ++i)
	{
		if (getPaths(input_ori, input_dest[i], output))
			return true;
	}
	return false;
}
bool
PathPlan::getPaths(LocationDH input_ori, LocationDH input_dest, std::vector<LocationDH>& output)
{

	LocationDH mid_pt_tool, mid_pt_rect;
	LocationDH input_ori_rect = input_ori;
	bool result = getMidPathPt(input_ori, input_dest, mid_pt_tool);
	if (!result)
		return false;
	mid_pt_rect = mid_pt_tool;
	tool2Rect(input_ori_rect.x, input_ori_rect.y, input_ori_rect.z, input_ori_rect.a, input_ori_rect.b, input_ori_rect.c);
	tool2Rect(mid_pt_rect.x, mid_pt_rect.y, mid_pt_rect.z, mid_pt_rect.a, mid_pt_rect.b, mid_pt_rect.c);
	std::vector<float> angles;
	if(!arm_ik_.getIKPos(input_ori_rect, angles))
		return false;
	LocationDH angle_ori;
	angle_ori.x = angles[0];
	angle_ori.y = angles[1];
	angle_ori.z = angles[2];
	angle_ori.a = angles[3];
	angle_ori.b = angles[4];
	angle_ori.c = angles[5];
	if(!arm_ik_.getIKPos(mid_pt_rect, angles))
		return false;
	LocationDH angle_mid;
	angle_mid.x = angles[0];
	angle_mid.y = angles[1];
	angle_mid.z = angles[2];
	angle_mid.a = angles[3];
	angle_mid.b = angles[4];
	angle_mid.c = angles[5];
	result = isReach(angle_ori, angle_mid, 1);
	if (!result)
		return false;
	output.resize(2);
	angle_mid.x = angle_mid.x * 180/CV_PI;
	angle_mid.y = angle_mid.y * 180 / CV_PI;
	angle_mid.z = angle_mid.z * 180 / CV_PI;
	angle_mid.a = angle_mid.a * 180 / CV_PI;
	angle_mid.b = angle_mid.b * 180 / CV_PI;
	angle_mid.c = angle_mid.c * 180 / CV_PI;

	output[0] = angle_mid;
	result = isReach(mid_pt_tool, input_dest, 0);
	if (!result)
		return false;
	output[1] = input_dest;
	return true;
}
bool
PathPlan::getMidPathPt(LocationDH input_ori, LocationDH input_dest, LocationDH& output)
{
	float mx, my, mz, rx, ry, rz;
	mx = input_dest.x;
	my = input_dest.y;
	mz = input_dest.z;
	rx = input_dest.a;
	ry = input_dest.b;
	rz = input_dest.c;
	cv::Mat R = cv::Mat(cv::Size(3, 3), CV_32FC1);
	rxryrz2Rotation(rx, ry, rz, R);
	cv::Mat clowDir = cv::Mat(cv::Size(1, 3), CV_32FC1);
	clowDir.at<float>(0, 0) = 0.f;
	clowDir.at<float>(1, 0) = 0.f;
	clowDir.at<float>(2, 0) = 1.f;
	cv::Mat tclowDir = R*clowDir;
	float length = 150.f;	//10cm

	output.x = input_dest.x - tclowDir.at<float>(0, 0)*length;
	output.y = input_dest.y - tclowDir.at<float>(1, 0)*length;
	output.z = input_dest.z - tclowDir.at<float>(2, 0)*length;
	output.a = input_dest.a;
	output.b = input_dest.b;
	output.c = input_dest.c;
	return true;
}
bool
PathPlan::isReach(LocationDH input_ori, LocationDH input_dest, int flag)
{
	if (flag == 0)
	{
		tool2Rect(input_ori.x, input_ori.y, input_ori.z, input_ori.a, input_ori.b, input_ori.c);
		tool2Rect(input_dest.x, input_dest.y, input_dest.z, input_dest.a, input_dest.b, input_dest.c);
	}

#ifdef _DEBUG
	pcl::PointCloud<pcl::PointXYZ>::Ptr collision_pts(new pcl::PointCloud<pcl::PointXYZ>);
	std::vector<LocationDH> insert_pts;
	getInsertPts(input_ori, input_dest, insert_pts, flag);
	for (int i = 0; i < insert_pts.size(); ++i)
	{
		if (!arm_ik_.isJointLimited(insert_pts[i], flag))
		{
			PathStatus ps;
			ps.stu = Status::JOINTLIMIT;
			ps.ori = input_ori;
			ps.dest = input_dest;
			path_status_.push_back(ps);
			return false;
		}

		if (flag == 0)
		{
			if (!coll_detect_.isCollision(insert_pts[i], flag, 1, collision_pts))
			{
				savePts("collision.asc", *collision_pts);
				PathStatus ps;
				ps.stu = Status::COLLISION;
				ps.ori = input_ori;
				ps.dest = input_dest;
				path_status_.push_back(ps);
				return false;
			}
		}
		else if (flag == 1)
		{
			if (!coll_detect_.isCollision(insert_pts[i], flag, 0, collision_pts))
			{
				savePts("collision.asc", *collision_pts);
				PathStatus ps;
				ps.stu = Status::COLLISION;
				ps.ori = input_ori;
				ps.dest = input_dest;
				path_status_.push_back(ps);
				return false;
			}
		}
	}
	PathStatus ps;
	ps.stu = Status::REACH;
	ps.ori = input_ori;
	ps.dest = input_dest;
	path_status_.push_back(ps);
	return true;

#else
	std::vector<LocationDH> insert_pts;
	getInsertPts(input_ori, input_dest, insert_pts, flag);
	for (int i = 0; i < insert_pts.size(); ++i)
	{
		if (!arm_ik_.isJointLimited(insert_pts[i], flag))
			return false;
		if (flag == 0)
		{
			if (!coll_detect_.isCollision(insert_pts[i], flag, 0))
			{
				return false;
			}
		}
		else if (flag == 1)
		{
			if (!coll_detect_.isCollision(insert_pts[i], flag, 1))
			{
				return false;
			}
		}
	}
	return true;
#endif

}
bool
PathPlan::setEnvirCloud(PointCloud& input_cloud)
{
	return coll_detect_.setEnvirCloud(input_cloud);
}
bool
PathPlan::setScanCloud(PointCloud& input_cloud)
{
	return coll_detect_.setScanCloud(input_cloud);
}
bool
PathPlan::loadConfigFile(std::string file_path)
{
	std::string ik_para = file_path + "\\arm_config.txt";
	std::string col_para = file_path + "\\grid.txt";
	std::string mesh_para = file_path + "\\Mesh.stl";
	if (!arm_ik_.setArmParam(ik_para.data()))
		return false;
	coll_detect_.setArmPara(arm_ik_);
	if (!coll_detect_.loadGridData(col_para.data()))
		if (!coll_detect_.initCompute(mesh_para))
			return false;
		else
			return true;
}